/**
 * @file           : send_process.cpp
 * @author         : fwy
 * @brief          :
 * @email          : 2643660853@qq.com
 * @date           : 23-1-18 上午2:25
 * @todo           : 需要用类封装
 */

#include <ros/ros.h>
#include "geometry_msgs/TwistStamped.h"
#include "msg_process/send_data.h"
#include "unitree_legged_msgs/MotorCmd.h"

#include "serial_data.h"
TransferData serial_send_data;

void lhipCallBack(const unitree_legged_msgs::MotorCmd l_hip_datas)
{
    serial_send_data.data_u8[0] = l_hip_datas.mode;
    serial_send_data.data_f[0] = l_hip_datas.q;
    serial_send_data.data_f[1] = l_hip_datas.dq;
    serial_send_data.data_f[2] = l_hip_datas.tau;
    serial_send_data.data_f[3] = l_hip_datas.Kp;
    serial_send_data.data_f[4] = l_hip_datas.Kd;
}

void lhip2CallBack(const unitree_legged_msgs::MotorCmd l_hip2_datas)
{
    serial_send_data.data_u8[1] = l_hip2_datas.mode;
    serial_send_data.data_f[5] = l_hip2_datas.q;
    serial_send_data.data_f[6] = l_hip2_datas.dq;
    serial_send_data.data_f[7] = l_hip2_datas.tau;
    serial_send_data.data_f[8] = l_hip2_datas.Kp;
    serial_send_data.data_f[9] = l_hip2_datas.Kd;
}

void lthighCallBack(const unitree_legged_msgs::MotorCmd l_thigh_datas)
{
    serial_send_data.data_u8[2] = l_thigh_datas.mode;
    serial_send_data.data_f[10] = l_thigh_datas.q;
    serial_send_data.data_f[11] = l_thigh_datas.dq;
    serial_send_data.data_f[12] = l_thigh_datas.tau;
    serial_send_data.data_f[13] = l_thigh_datas.Kp;
    serial_send_data.data_f[14] = l_thigh_datas.Kd;
}

void lcalfCallBack(const unitree_legged_msgs::MotorCmd l_calf_datas)
{
    serial_send_data.data_u8[3] = l_calf_datas.mode;
    serial_send_data.data_f[15] = l_calf_datas.q;
    serial_send_data.data_f[16] = l_calf_datas.dq;
    serial_send_data.data_f[17] = l_calf_datas.tau;
    serial_send_data.data_f[18] = l_calf_datas.Kp;
    serial_send_data.data_f[19] = l_calf_datas.Kd;
}

void ltoeCallBack(const unitree_legged_msgs::MotorCmd l_toe_datas)
{
    serial_send_data.data_u8[4] = l_toe_datas.mode;
    serial_send_data.data_f[20] = l_toe_datas.q;
    serial_send_data.data_f[21] = l_toe_datas.dq;
    serial_send_data.data_f[22] = l_toe_datas.tau;
    serial_send_data.data_f[23] = l_toe_datas.Kp;
    serial_send_data.data_f[24] = l_toe_datas.Kd;
}

void rhipCallBack(const unitree_legged_msgs::MotorCmd r_hip_datas)
{
    serial_send_data.data_u8[5] = r_hip_datas.mode;
    serial_send_data.data_f[25] = r_hip_datas.q;
    serial_send_data.data_f[26] = r_hip_datas.dq;
    serial_send_data.data_f[27] = r_hip_datas.tau;
    serial_send_data.data_f[28] = r_hip_datas.Kp;
    serial_send_data.data_f[29] = r_hip_datas.Kd;
}

void rhip2CallBack(const unitree_legged_msgs::MotorCmd r_hip2_datas)
{
    serial_send_data.data_u8[6] = r_hip2_datas.mode;
    serial_send_data.data_f[30] = r_hip2_datas.q;
    serial_send_data.data_f[31] = r_hip2_datas.dq;
    serial_send_data.data_f[32] = r_hip2_datas.tau;
    serial_send_data.data_f[33] = r_hip2_datas.Kp;
    serial_send_data.data_f[34] = r_hip2_datas.Kd;
}

void rthighCallBack(const unitree_legged_msgs::MotorCmd r_thigh_datas)
{
    serial_send_data.data_u8[7] = r_thigh_datas.mode;
    serial_send_data.data_f[35] = r_thigh_datas.q;
    serial_send_data.data_f[36] = r_thigh_datas.dq;
    serial_send_data.data_f[37] = r_thigh_datas.tau;
    serial_send_data.data_f[38] = r_thigh_datas.Kp;
    serial_send_data.data_f[39] = r_thigh_datas.Kd;
}


void rcalfCallBack(const unitree_legged_msgs::MotorCmd r_calf_datas)
{
    serial_send_data.data_u8[8] = r_calf_datas.mode;
    serial_send_data.data_f[40] = r_calf_datas.q;
    serial_send_data.data_f[41] = r_calf_datas.dq;
    serial_send_data.data_f[42] = r_calf_datas.tau;
    serial_send_data.data_f[43] = r_calf_datas.Kp;
    serial_send_data.data_f[44] = r_calf_datas.Kd;
}

void rtoeCallBack(const unitree_legged_msgs::MotorCmd r_toe_datas)
{
    serial_send_data.data_u8[9] = r_toe_datas.mode;
    serial_send_data.data_f[45] = r_toe_datas.q;
    serial_send_data.data_f[46] = r_toe_datas.dq;
    serial_send_data.data_f[47] = r_toe_datas.tau;
    serial_send_data.data_f[48] = r_toe_datas.Kp;
    serial_send_data.data_f[49] = r_toe_datas.Kd;
}

int main(int argc, char **argv)
{
    // 初始化节点
    ros::init(argc, argv, "send_process");
    ros::NodeHandle nh;
    // 数据初始化
    serial_send_data.data_u8 = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    serial_send_data.data_f = {
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    ros::Subscriber L_hip_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("L_hip_controller/command", 1, lhipCallBack);
    ros::Subscriber L_hip2_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("L_hip2_controller/command", 1, lhip2CallBack);
    ros::Subscriber L_thigh_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("L_thigh_controller/command", 1, lthighCallBack);
    ros::Subscriber L_calf_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("L_calf_controller/command", 1, lcalfCallBack);
    ros::Subscriber L_toe_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("L_toe_controller/command", 1, ltoeCallBack);
    ros::Subscriber R_hip_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("R_hip_controller/command", 1, rhipCallBack);
    ros::Subscriber R_hip2_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("R_hip2_controller/command", 1, rhip2CallBack);
    ros::Subscriber R_thigh_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("R_thigh_controller/command", 1, rthighCallBack);
    ros::Subscriber R_calf_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("R_calf_controller/command", 1, rcalfCallBack);
    ros::Subscriber R_toe_sub = nh.subscribe<unitree_legged_msgs::MotorCmd>("R_toe_controller/command", 1, rtoeCallBack);
    ros::Rate rate(100);
    while (ros::ok())
    {
        serialdata.write(serial_send_data);
        ros::spinOnce();
        rate.sleep();
    }
}
